#include <QString>
#include <QThread>
#include "controller.h"
#include "rs232.h"
#include "log4z.h"

using namespace zsummer::log4z;
const int maxTm = 7;
const int slpTm = 500;
const int itlTm = 100;

extern void* errBeep(void* ptr);

Controller::Controller()
{

}

int Controller::SetLightPar(QString iVl, QString iPulse, QString iInt, QString iSpeed)
{
    int cport_nr=0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate=57600;       /* 57600 baud */
    //HP2PAR
    char buf[8] = {0x01, 0x02, 0x48, 0x50, 0x32, 0x50, 0x41, 0x52};

    //append data
    std::string strAppend = iVl.toStdString() + "," + iPulse.toStdString();
    strAppend += "," + iInt.toStdString() + "," + iSpeed.toStdString();
    int iDataSize = strAppend.size();

    LOGFMTD("Func %s: Params: %s", __func__, strAppend.c_str());

    char* newBuf = new char[9+iDataSize];
    memset(newBuf, 0, 9+iDataSize);
    strncpy(newBuf, buf, 8);
    strncpy(newBuf+8, strAppend.c_str(), iDataSize);
    newBuf[8+iDataSize] = 0x03;

    char mode[]={'8','N','1',0};

    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    //clear
    RS232_flushRXTX(cport_nr);
    QThread::msleep(100);
    RS232_SendBuf(cport_nr, (unsigned char*)newBuf, 9+strAppend.size());
    QThread::msleep(itlTm);
    delete[] newBuf;
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);

    bool bRead = true;
    int iTm = 0, iTotalDt= 0;
    while(bRead && iTotalDt < 9)
    {
        if (iCnt > 0)
        {
            iRet = 1;
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());

#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::LightShock()
{
    int cport_nr=0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate=57600;       /* 57600 baud */
    //SHOCHK
    char buf[9] = {0x01, 0x02, 0x53, 0x48, 0x4F, 0x43, 0x48, 0x4B, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    //clear
    RS232_flushRXTX(cport_nr);
    QThread::msleep(100);
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    QThread::msleep(itlTm);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 9)
    {
        if (iCnt > 0)
        {
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm*3)
        {
            //No data
            if (iTotalDt == 0)
            {
                iRet = -2;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());

    //OK
    if ((readBuf[6] == 0x31 && readBuf[7] ==0x30) || (readBuf[6] == 0x32 && readBuf[7] ==0x30))
    {
        iRet = 1;
    }
    else if (readBuf[6] == 0x31 && readBuf[7] ==0x31)
    {
        iRet = -1;
    }
    else if (readBuf[6] == 0x30 && readBuf[7] ==0x30)//no guan
    {
        iRet = -3;
    }
    else if (readBuf[6] == 0x30 && readBuf[7] ==0x31)//power error
    {
        iRet = -4;
    }
    else if (readBuf[6] == 0x30 && readBuf[7] ==0x32)//IGBT error
    {
        iRet = -5;
    }
    else
    {
        iRet = -2;
    }
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::StartLight()
{
    int cport_nr=0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate=57600;       /* 57600 baud */
    int iRet = 0;
    //HP2STR
    char buf[9] = {0x01, 0x02, 0x48, 0x50, 0x32, 0x53, 0x54, 0x52, 0x03};
    char mode[]={'8','N','1',0};
    LOGFMTD("Func %s:", __func__);
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    //clear
    RS232_flushRXTX(cport_nr);
    QThread::msleep(100);
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    iRet = 1;

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::StopLight()
{
//    bRunningData = false;
//    if (readData != 0)
//    {
//        readData->Wait();
//        delete readData;
//        readData = 0;
//    }
    int cport_nr=0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate=57600;       /* 57600 baud */
    //HP2STP
    char buf[9] = {0x01, 0x02, 0x48, 0x50, 0x32, 0x53, 0x54, 0x50, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    //clear
    RS232_flushRXTX(cport_nr);
    QThread::msleep(100);
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0, iLp = 0, iFnd = 0;
    while(bRead && iTotalDt < 128)
    {
        if (iCnt > 0)
        {
            iRet = 1;
            iTotalDt += iCnt;
            while (iLp < iTotalDt-1)
            {
                if (readBuf[iLp] == 'T' && readBuf[iLp+1] == 'P')
                {
                    LOGD("Found TP at buf...");
                    iFnd = 1;
                    break;
                }
                iLp++;
            }
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());
    if (iFnd == 1 && readBuf[6] == 0x31)
    {
        iRet = 1;
    }
    else
    {
        iRet = -1;
    }
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif
    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::CheckLight()
{
    int cport_nr=0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate=57600;       /* 57600 baud */

    //DETECT
    char buf[9] = {0x01, 0x02, 0x44, 0x45, 0x54, 0x45, 0x43, 0x54, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    //clear
    RS232_flushRXTX(cport_nr);
    QThread::msleep(100);
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 9)
    {
        if (iCnt > 0)
        {
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm*3)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());

    if (readBuf[7] == 0x31) //error
    {
        iRet = -2;
    }
    else if (readBuf[7] == 0x32) //error
    {
        iRet = -3;
    }
    else if (readBuf[7] == 0x30)
    {
        iRet = 1;
    }
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::StartRuDong()
{
    int cport_nr = 0,    /* /dev/ttyS1 (COM1 on windows) */
    bdrate= 57600;       /* 57600 baud */

    char buf[9] = {0x01, 0x02, 0x52, 0x44, 0x42, 0x53, 0x54, 0x52, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    //clear
    RS232_flushRXTX(cport_nr);
    QThread::msleep(100);
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    QThread::msleep(200);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 8)
    {
        if (iCnt > 0)
        {
            iRet = 1;
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::StopRuDong()
{
    int cport_nr = 0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate= 57600;       /* 57600 baud */

    char buf[9] = {0x01, 0x02, 0x52, 0x44, 0x42, 0x53, 0x54, 0x50, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    //clear
    RS232_flushRXTX(cport_nr);
    QThread::msleep(100);
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    QThread::msleep(200);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 8)
    {
        if (iCnt > 0)
        {
            iRet = 1;
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::OpenDoor()
{
    int cport_nr = 0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate= 57600;       /* 57600 baud */

    char buf[9] = {0x01, 0x02, 0x47, 0x41, 0x54, 0x4F, 0x50, 0x4E, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    QThread::msleep(200);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 8)
    {
        if (iCnt > 0)
        {
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm*5)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());

    if (readBuf[6] == 0x31)
    {
        iRet = 1;
    }
    else
    {
        iRet = -1;
    }
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif
    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::CloseDoor()
{
    int cport_nr = 0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate= 57600;       /* 57600 baud */

    char buf[9] = {0x01, 0x02, 0x47, 0x41, 0x54, 0x43, 0x4C, 0x45, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    QThread::msleep(200);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 8)
    {
        if (iCnt > 0)
        {
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm*5)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());

    if (readBuf[6] == 0x31)
    {
        iRet = 1;
    }
    else
    {
        iRet = -1;
    }
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::OpenYao()
{
    int cport_nr = 0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate= 57600;       /* 57600 baud */

    char buf[9] = {0x01, 0x02, 0x53, 0x48, 0x41, 0x4F, 0x50, 0x4E, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    QThread::msleep(200);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 8)
    {
        if (iCnt > 0)
        {
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm*10)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());

    if (readBuf[6] == 0x31)
    {
        iRet = 1;
    }
    else
    {
        iRet = -1;
    }
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif
    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::CloseYao()
{
    int cport_nr = 0,     /* /dev/ttyS1 (COM1 on windows) */
    bdrate= 57600;       /* 57600 baud */

    char buf[9] = {0x01, 0x02, 0x53, 0x48, 0x41, 0x53, 0x54, 0x50, 0x03};
    char mode[]={'8','N','1',0};

    LOGFMTD("Func %s:", __func__);
    int iRet = 0;
#ifdef INTERNAL_OPEN
    if(RS232_OpenComport(cport_nr, bdrate, mode))
    {
        LOGFMTE("Open Serial Failed!");
        return iRet;
    }
#endif
    RS232_SendBuf(cport_nr, (unsigned char*)buf, 9);
    QThread::msleep(200);
    //read data
    unsigned char readBuf[128];
    memset(readBuf, 0, 128);
    int iCnt = RS232_PollComport(cport_nr, readBuf, 127);
    bool bRead = true;
    int iTm = 0, iTotalDt = 0;
    while(bRead && iTotalDt < 8)
    {
        if (iCnt > 0)
        {
            iTotalDt += iCnt;
        }
        else
        {
            iTm++;
        }
        if (iTm >= maxTm*10)
        {
            if (iTotalDt == 0)
            {
                iRet = -1;
            }
            break;
        }
        QThread::msleep(slpTm);
        iCnt = RS232_PollComport(cport_nr, readBuf+iTotalDt, 127-iTotalDt);
    }
    std::string strBuf(reinterpret_cast<const char*>(readBuf), iTotalDt);
    LOGFMTD("Func %s: Size=%d, Buff=%s", __func__, iTotalDt, strBuf.c_str());

    if (readBuf[6] == 0x31)
    {
        iRet = 1;
    }
    else
    {
        iRet = -1;
    }
#ifdef INTERNAL_OPEN
    RS232_CloseComport(cport_nr);
#endif

    LOGFMTD("Return result=%d", iRet);
    return iRet;
}

int Controller::CanLiuIo()
{
    char buf[16];
    memset(buf,0,16);
#if defined(__linux__) || defined(__FreeBSD__)
    int fd = open( "/sys/class/gpio/gpio48/value", O_RDONLY | O_NONBLOCK );
    int iSz = read(fd,buf,sizeof(buf));
    if (iSz == -1)
    {
        return -1;
    }
    close(fd);
#endif
    int iData = atoi(buf);
    return iData;
}

int Controller::StartIo()
{
    char buf[16];
    memset(buf,0,16);
#if defined(__linux__) || defined(__FreeBSD__)
    int fd = open( "/sys/class/gpio/gpio30/value", O_RDONLY | O_NONBLOCK );
    int iSz = read(fd,buf,sizeof(buf));
    if (iSz == -1)
    {
        return -1;
    }
    close(fd);
#endif
    int iData = atoi(buf);
    return iData;
}

int Controller::StopIo()
{
    char buf[16];
    memset(buf,0,16);
#if defined(__linux__) || defined(__FreeBSD__)
    int fd = open( "/sys/class/gpio/gpio31/value", O_RDONLY | O_NONBLOCK );
    int iSz = read(fd,buf,sizeof(buf));
    if (iSz == -1)
    {
        return -1;
    }
    close(fd);
#endif
    int iData = atoi(buf);
    return iData;
}

int Controller::ErrorIo()
{
#if defined(__linux__) || defined(__FreeBSD__)
    //one thread to control the beeper
    pthread_t beep_tid;
    int err = pthread_create(&beep_tid, NULL, errBeep, 0);
#endif
    return 1;
}

int Controller::NormalIo()
{
#if defined(__linux__) || defined(__FreeBSD__)
    system("echo 0 > /sys/class/gpio/gpio60/value");
    system("echo 0 > /sys/class/gpio/gpio113/value");
    system("echo 0 > /sys/class/gpio/gpio111/value");
    system("echo 1 > /sys/class/gpio/gpio112/value");
#endif
    return 1;
}

int Controller::PowerIo()
{
    char buf[16];
    memset(buf,0,16);
#if defined(__linux__) || defined(__FreeBSD__)
    int fd = open( "/sys/class/gpio/gpio3/value", O_RDONLY | O_NONBLOCK );
    int iSz = read(fd,buf,sizeof(buf));
    if (iSz == -1)
    {
        return -1;
    }
    close(fd);
#endif
    int iData = atoi(buf);
    return iData;
}

int Controller::SpeedMotor(int iSpeed)
{
#if defined(__linux__) || defined(__FreeBSD__)
    if (iSpeed == 1)
    {
        system("echo 200000 > /sys/class/pwm/pwmchip0/pwm0/duty_cycle");
        system("echo 200000 > /sys/class/pwm/pwmchip0/pwm1/duty_cycle");
    }
    else if (iSpeed == 0)
    {
        system("echo 0 > /sys/class/pwm/pwmchip0/pwm0/duty_cycle");
        system("echo 0 > /sys/class/pwm/pwmchip0/pwm1/duty_cycle");

    }
    else if (iSpeed == 2)
    {
        system("echo 800000 > /sys/class/pwm/pwmchip0/pwm0/duty_cycle");
        system("echo 800000 > /sys/class/pwm/pwmchip0/pwm1/duty_cycle");
    }
#endif
    return 1;
}

int Controller::StartMotor()
{
#if defined(__linux__) || defined(__FreeBSD__)
    system("echo 1 > /sys/class/pwm/pwmchip0/pwm0/enable");
    system("echo 1 > /sys/class/pwm/pwmchip0/pwm1/enable");
#endif
    return 1;
}

int Controller::StopMotor()
{
#if defined(__linux__) || defined(__FreeBSD__)
    system("echo 0 > /sys/class/pwm/pwmchip0/pwm0/enable");
    system("echo 0 > /sys/class/pwm/pwmchip0/pwm1/enable");
#endif
    return 1;
}
